#!/usr/bin/env python

import rospy
import sys
import moveit_commander


class MoveItFkDemo:
	def __init__(self):
		moveit_commander.roscpp_initialize(sys.argv)
		rospy.init_node("moveit_fk_demo")
		arm = moveit_commander.MoveGroupCommander("xarm")
		arm.set_max_velocity_scaling_factor(1.0)  
		gripper = moveit_commander.MoveGroupCommander("gripper")
		gripper.set_max_velocity_scaling_factor(1.0)  

		joint_positions = [0, 0, 0, 0, 0, 0]
		arm.set_joint_value_target(joint_positions)
		plan_success, traj, planning_time, error_code  = arm.plan()
		arm.execute(traj)
		#arm.go()
		rospy.sleep(1)
		
		joint_positions = [-0.664, -0.775, 0.675, -1.241, -0.473, -1.281]
		arm.set_joint_value_target(joint_positions)
		#arm.go()
		
		plan_success, traj, planning_time, error_code  = arm.plan()
		arm.execute(traj)
		rospy.sleep(1)
		joint_positions = [0.65, 0.65]
		gripper.set_joint_value_target(joint_positions)
		gripper.go()
		rospy.sleep(1)
		joint_positions = [0.0, 0.0]
		gripper.set_joint_value_target(joint_positions)
		gripper.go()
		rospy.sleep(1)
		arm.set_named_target("Home")
		arm.go()
		rospy.sleep(1)

		moveit_commander.roscpp_shutdown()
		moveit_commander.os._exit(0)

if __name__ == "__main__":
	try:
		MoveItFkDemo()
	except rospy.ROSInterruptException:
		pass